#include "MinkowskiDifference2.h"
#include "math.h"
#include "IDomain2.h"


MinkowskiDifference2::InternalPoint::InternalPoint()
: pointOn1_(), pointOn2_(), point_()
{
}

MinkowskiDifference2::InternalPoint::InternalPoint(const MinkowskiDifference2& difference)
: pointOn1_(transformedInternalPoint(*difference.p1, *difference.t1))
, pointOn2_(transformedInternalPoint(*difference.p2, *difference.t2))
, point_(pointOn2_ - pointOn1_)
{
}

MinkowskiDifference2::SupportPoint::SupportPoint()
: normal_(), pointOn1_(), pointOn2_(), point_()
{
}

MinkowskiDifference2::SupportPoint::SupportPoint(const MinkowskiDifference2& difference, const Vector2& normal)
: normal_(normal)
, pointOn1_(transformedSupportPoint(*difference.p1, *difference.t1, -normal))
, pointOn2_(transformedSupportPoint(*difference.p2, *difference.t2, normal))
, point_(pointOn2_ - pointOn1_)
{
}

////////////////////////////////////////
MinkowskiDifference2::MinkowskiDifference2()
	: p1(), t1(), p2(), t2()
{
}

MinkowskiDifference2::MinkowskiDifference2(const IConvexDomain2& p1, const Vector2& t1, const IConvexDomain2& p2, const Vector2& t2)
	: p1(&p1), t1(&t1), p2(&p2), t2(&t2)
{}

const Vector2 MinkowskiDifference2::position() const
{
	return *t2 - *t1;
}

void MinkowskiDifference2::reset(const IConvexDomain2& domainA, const Vector2& positionA, const IConvexDomain2& domainB, const Vector2& positionB)
{
	p1 = (&domainA);
	t1 = (&positionA);
	p2 = (&domainB);
	t2 = (&positionB);
}

const Vector2 MinkowskiDifference2::internalPoint() const
{
	InternalPoint result;
	internalPointEx(result);
	return result.point();
}

void MinkowskiDifference2::internalPointEx(InternalPoint& result) const
{
	result = InternalPoint(*this);
}

const Vector2 MinkowskiDifference2::supportPoint(const Vector2& normal) const
{
	SupportPoint result;
	supportPointEx(normal, result);
	return result.point();
}

void MinkowskiDifference2::supportPointEx(const Vector2& normal, SupportPoint& result) const
{
	result = SupportPoint(*this, normal);
}